/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "scene/include/hv_in_map.h"

#include <cmath>
#include <vector>

#include <glog/logging.h>

#include "v2xpb-asn-bsm.pb.h"
#include "v2xpb-asn-map.pb.h"
#include "v2xpb-asn-position.pb.h"
#include "v2xpb-asn-spat.pb.h"

namespace v2x {
namespace scene {
const ::v2xpb::asn::MapNode& HvInMap::GetMapNode() { return map_node_; }
const ::v2xpb::asn::MapLink& HvInMap::GetMapLink() {
  return map_node_.links(link_index_);
}
const ::v2xpb::asn::MapLane& HvInMap::GetMapLane() {
  return map_node_.links(link_index_).lanes(lane_index_);
}
double HvInMap::CalculateDistance(const ::v2xpb::asn::Position& point_start,
                           const ::v2xpb::asn::Position& point_end,
                           const ::v2xpb::asn::Position& pose) {
  double start_to_pose_x = point_start.xyz().x() - pose.xyz().x();
  double start_to_pose_y = point_start.xyz().y() - pose.xyz().y();
  double end_to_pose_x = point_end.xyz().x() - pose.xyz().x();
  double end_to_pose_y = point_end.xyz().y() - pose.xyz().y();
  double double_triangle_square =
    (start_to_pose_x * end_to_pose_y) - (end_to_pose_x * start_to_pose_y);
  double delta_x = point_start.xyz().x() - point_end.xyz().x();
  double delta_y = point_start.xyz().y() - point_end.xyz().y();
  double start_to_end_distance =
    std::sqrt(delta_x * delta_x + delta_y * delta_y);
  double distance = std::fabs(double_triangle_square / start_to_end_distance);
  LOG(INFO) << "distance between position and hv is " << distance;
  return distance;
}
bool HvInMap::IsHvInMap(const ::v2xpb::asn::Map& map,
                        const ::v2xpb::asn::Position& pose, double heading) {
  LOG(INFO) << "map node size : " << map.nodes_size();
  for (int i = 0; i < map.nodes_size(); ++i) {
    // map node
    LOG(INFO) << "current node index: " << i;
    const auto& intersection = map.nodes(i);
    for (int j = 0; j < intersection.links_size(); ++j) {
      // link
      LOG(INFO) << "current link index: " << j;
      if (IsHvInLink(intersection, j, pose, heading)) {
        return true;
      }
    }
  }
  return false;
}

bool HvInMap::IsHvInLink(const ::v2xpb::asn::MapNode& node, int link_id,
                         const ::v2xpb::asn::Position& pose, double heading) {
  const auto& link = node.links(link_id);
  for (int i = 0; i < link.lanes_size(); ++i) {
    LOG(INFO) << "current lane index: " << i;
    if (IsHvInLane(node, link_id, i, pose, heading)) {
      return true;
    }
  }
  return false;
}
bool HvInMap::IsHvInLane(const ::v2xpb::asn::MapNode& node, int link_id,
                         int lane_id, const ::v2xpb::asn::Position& pose,
                         double heading) {
  const auto& lane = node.links(link_id).lanes(lane_id);
  double half_lane_width = kDefaultLaneWidth;
  if (lane.has_width()) {
    half_lane_width = lane.width() * 0.5;
    LOG(INFO) << "lane width is " << half_lane_width;
  } else {
    LOG(WARNING) << "lane width is none, use default value " << half_lane_width;
  }
  LOG(INFO) << "lane positions size is " << lane.positions_size();
  if (lane.positions_size() < 2) {
    LOG(ERROR) << "lane positions_size is less than 2, return false";
    return false;
  }
  int pose_size = lane.positions_size();
  ::v2xpb::asn::Position last_point = lane.positions(pose_size - 1);
  ::v2xpb::asn::Position last_2nd_point = lane.positions(pose_size - 2);
  double delta_y = pose.xyz().y() - last_point.xyz().y();
  double delta_x = pose.xyz().x() - last_point.xyz().x();
  double angle_hv_lane = atan2(delta_y, delta_x);
  delta_y = last_2nd_point.xyz().y() - last_point.xyz().y();
  delta_x = last_2nd_point.xyz().x() - last_point.xyz().x();
  double angle_lane = atan2(delta_y, delta_x);
  if (std::fabs(angle_hv_lane) > std::numeric_limits<double>::epsilon() &&
    std::fabs(angle_lane) > std::numeric_limits<double>::epsilon() &&
    std::fabs(std::fabs(angle_lane) - std::fabs(angle_hv_lane)) > M_PI_2) {
    LOG(WARNING) << "host vehicle is outside of the lane, return false";
    return false;
  }
  if (lane.positions_size() >= 3) {
    for (int i = 0; i < pose_size - 2; ++i) {
      ::v2xpb::asn::Position start_point = lane.positions(i);
      ::v2xpb::asn::Position middle_point = lane.positions(i + 1);
      ::v2xpb::asn::Position end_point = lane.positions(i + 2);
      double distance = CalculateDistance(start_point, end_point, middle_point);
      if (distance > half_lane_width) {
        LOG(WARNING) << "the lane has bend part, return false";
        return false;
      }
    }
  }
  for (int i = 0; i < pose_size - 1; ++i) {
    ::v2xpb::asn::Position start_point = lane.positions(i);
    ::v2xpb::asn::Position end_point = lane.positions(i + 1);
    LOG(INFO) << "lane pose index: " << i;
    double distance = CalculateDistance(start_point, end_point, pose);
    if (distance < half_lane_width) {
      double delta_y = end_point.xyz().y() - start_point.xyz().y();
      double delta_x = end_point.xyz().x() - start_point.xyz().x();
      double line_azimuth = atan2(delta_y, delta_x);
      double azimuth_diff = line_azimuth - heading;
      LOG(INFO) << "line_azimuth is " << line_azimuth << ", hv heading is "
                << heading << " azimuth_diff is " << azimuth_diff;
      azimuth_diff = std::fabs(azimuth_diff);
      LOG(INFO) << "kAzimuthThrehold is " << kAzimuthThrehold;
      if (azimuth_diff < kAzimuthThrehold) {
        LOG(INFO) << "hv is likely to be on this lane, return true";
        LaneDistance lane_distance;
        lane_distance.node_ = node;
        lane_distance.link_id_ = link_id;
        lane_distance.lane_id_ = lane_id;
        lane_distance.distance_ = distance;
        lane_distance_.emplace_back(lane_distance);
        return true;
      }
    }
  }
  return false;
}

bool HvInMap::FindNearestLane() {
  if (lane_distance_.size() == 0) {
    LOG(ERROR) << "can not find the lane which host vehicle on";
    return false;
  }
  double min_distance = std::numeric_limits<double>::max();
  for (int i = 0; i < lane_distance_.size(); ++i) {
    if (lane_distance_[i].distance_ < min_distance) {
      min_distance = lane_distance_[i].distance_;
      map_node_ = lane_distance_[i].node_;
      link_index_ = lane_distance_[i].link_id_;
      lane_index_ = lane_distance_[i].lane_id_;
    }
  }
  LOG(INFO) << "the distance on the nearest lane is " << min_distance;
  return true;
}
}  // namespace scene
}  // namespace v2x
